#include <iostream>
#include <string>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/se3.hpp"
#include "src/JPLQuatOps.h"
#include "src/HamiltonQuatOps.h"

using namespace std;
using namespace Eigen;

void print_quat(std::string title, Eigen::Matrix<double, 4, 1> q) {
    cout << title << q(0, 0) << " " << q(1, 0) << " " 
        << q(2, 0) << " " << q(3, 0) << endl;
}

void print_Quaterniond(std::string title, Quaterniond q) {
    cout << title << q.x() << " y:" << q.y() << " z:" 
        << q.z() << " w:" << q.w() << endl;
}

int main(int argc, char **argv) {
    // 沿Z轴转90度的旋转矩阵
    Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
    // 或者四元数
    // Quaterniond q(0, 0, 0, 1);
    // q.normalize();
    // Matrix3d R = q.toRotationMatrix();
    Quaterniond q(R); // x, y, z, w
    Sophus::SO3d SO3_R(R);
 
    auto HQ0 = xvslam::HamiltonQuatOps::rot_2_quat(R);
    auto JQ0 = xvslam::JPLQuatOps::rot_2_quat(R);
    
    cout << "------------- check rot_2_quat -----------" << endl;
    print_Quaterniond("q = ", q);
    print_quat("HQ0 = ", HQ0);
    print_quat("JQ0 = ", JQ0);

    cout << "------------- check quat_2_rot -----------" << endl;
    cout << "qR = " << q.toRotationMatrix() << endl;
    cout << "HR = " << xvslam::HamiltonQuatOps::quat_2_rot(HQ0).matrix()  << endl;
    cout << "JR = " << xvslam::JPLQuatOps::quat_2_rot(JQ0).matrix()  << endl;


    cout << "------------- check quat_multiply -----------" << endl;
    Eigen::Matrix<double, 3, 1> v(0, 3e-4, 0); // x, y, z
    auto v_rotated = q * v;
    // Eigen会默认直接赋值法的存储顺序为wxyz 
    // https://blog.csdn.net/shy2333/article/details/130583927
    auto v_equal = q * Quaterniond(0, v.x(), v.y(), v.z()) * q.inverse();
    cout << "v_rotated = " << v_rotated.transpose() << endl;
    cout << "v_equal = " << v_equal.coeffs().transpose() << endl; // x y z w

    cout << "------------- check update -----------" << endl;
    Eigen::Matrix<double, 4, 1> vq;
    vq.block(0, 0, 3, 1) = v;
    vq(3, 0) = 1.0f;
    Sophus::SO3d SO3_update = Sophus::SO3d::exp(v) * SO3_R;
    Quaterniond q_update(SO3_update.matrix());
    vq.block(0, 0, 3, 1) = 0.5 * vq.block(0, 0, 3, 1);
    auto v_hamiltion = xvslam::HamiltonQuatOps::quat_multiply(vq, HQ0);
    auto v_jpl = xvslam::JPLQuatOps::quat_multiply(vq, JQ0);
    print_Quaterniond("q_update = ", q_update);
    print_quat("HV = ", v_hamiltion);
    print_quat("JV = ", v_jpl);
    return 0;
}